#include <AccelStepper.h>

// --- STRUTTURA DATI PER IL DEBOUNCE ---
struct Pulsante {
  int pin;
  int statoFiltrato;
  int ultimoStatoLettura;
  unsigned long ultimoTempoDebounce;
};

// --- CONFIGURAZIONE PIN E MOTORI ---
// Motori (Interfaccia driver 1 = STEP/DIR)
AccelStepper motori[4] = {
  AccelStepper(1, 2, 3),  // Motore A: Step 2, Dir 3
  AccelStepper(1, 4, 5),  // Motore B: Step 4, Dir 5
  AccelStepper(1, 6, 7),  // Motore C: Step 6, Dir 7
  AccelStepper(1, 8, 9)   // Motore D: Step 8, Dir 9
};

// Inizializzazione Pulsanti {Pin, StatoFiltrato, UltimoStato, Tempo}
// 2 pulsanti per ogni motore (Orario e Antiorario)
Pulsante btnCW[4] = {
  {A0, HIGH, HIGH, 0}, {A2, HIGH, HIGH, 0}, {10, HIGH, HIGH, 0}, {12, HIGH, HIGH, 0}
};

Pulsante btnCCW[4] = {
  {A1, HIGH, HIGH, 0}, {A3, HIGH, HIGH, 0}, {11, HIGH, HIGH, 0}, {13, HIGH, HIGH, 0}
};

// Variabili Timer
unsigned long timerSeriale = 0;
const int velocitaMotore = 600; // Impulsi al secondo

void setup() {
  Serial.begin(9600);
  
  for (int i = 0; i < 4; i++) {
    pinMode(btnCW[i].pin, INPUT_PULLUP);
    pinMode(btnCCW[i].pin, INPUT_PULLUP);
    motori[i].setMaxSpeed(1000);
  }
  Serial.println("Sistema Pronto. Controllo 4 motori attivo.");
}

void loop() {
  // --- 1. GESTIONE MOVIMENTO E FILTRO ---
  for (int i = 0; i < 4; i++) {
    // Leggiamo lo stato filtrato dei due pulsanti per il motore i
    int orario = leggiPulsanteFiltrato(btnCW[i]);
    int antiorario = leggiPulsanteFiltrato(btnCCW[i]);

    if (orario == LOW) {
      motori[i].setSpeed(velocitaMotore);
      motori[i].runSpeed();
    } 
    else if (antiorario == LOW) {
      motori[i].setSpeed(-velocitaMotore);
      motori[i].runSpeed();
    } 
    else {
      motori[i].setSpeed(0);
      // Nota: non serve chiamare runSpeed() se la velocità è 0
    }
  }

  // --- 2. STAMPA SERIALE OGNI 2 SECONDI ---
  if (millis() - timerSeriale >= 2000) {
    timerSeriale = millis();
    stampaStatoPulsanti();
  }
}

// --- FUNZIONE UNIVERSALE DI FILTRO ---
int leggiPulsanteFiltrato(Pulsante &p) {
  int lettura = digitalRead(p.pin);

  if (lettura != p.ultimoStatoLettura) {
    p.ultimoTempoDebounce = millis();
  }

  if ((millis() - p.ultimoTempoDebounce) > 50) { // 50ms di filtro
    p.statoFiltrato = lettura;
  }

  p.ultimoStatoLettura = lettura;
  return p.statoFiltrato;
}

// --- FUNZIONE DI STAMPA LOG ---
void stampaStatoPulsanti() {
  Serial.println("\n--- Report Stato Pulsanti ---");
  char nomi[] = {'A', 'B', 'C', 'D'};
  for (int i = 0; i < 4; i++) {
    Serial.print("Motore "); Serial.print(nomi[i]); Serial.print(": ");
    if (btnCW[i].statoFiltrato == LOW) Serial.print("PREMUTO CW  ");
    else Serial.print("libero CW   ");
    
    if (btnCCW[i].statoFiltrato == LOW) Serial.println("| PREMUTO CCW");
    else Serial.println("| libero CCW");
  }
}